/*
 * Utils.h
 *
 */

#ifndef UTILS_H_
#define UTILS_H_

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <iostream>

#include "vision/POI.h"
#include "Config.h"

using namespace cv;

namespace utils {

/**
 * \class Utils
 * @brief
 * @details
 * @date May 25, 2012
 * @author Gennaro Capo
 * @author Mirko Conte
 * @author Vincenzo De Notaris
 * @author Roberto Pacilio
 * @version 1.0
 * @namespace utils
 */
class Utils {

public:

	/**
	 *
	 * @param keypoint
	 * @param label
	 * @param distance
	 * @return
	 */
	inline static poi::POI* KeyPoint2POI(const cv::KeyPoint &keypoint, ObjectType label, float distance = 0) {
		return new poi::POI(keypoint.pt, label, distance);
	}

	/**
	 *
	 * @param poi
	 */
	inline static void printPOI(const poi::POI &poi) {
		std::cout << "Keypoint found! - (" << poi.getPoint().x << ", " << poi.getPoint().y << ")\t- Angle: "<< poi.getAngle() << "\t-\tDist:\t" << poi.getDistance() << "\t-\tType:\t" << poi.getLabel() << std::endl;
	}

	/**
	 *
	 * @param depth
	 * @param point
	 * @return
	 */
	inline static float calcDistanceFromDepthMat(const cv::Mat &depth, const cv::Point2f &point) {
		return 0.1236 * tan(depth.at<unsigned short>(point) / 2842.5 + 1.1863) * 1000;
	}

	/**
	 *
	 * @param point
	 * @return
	 */
	inline static float calcAngle(const cv::Point2f &point) {
		return (point.x * SCALE_FACTOR) - (HORIZONTAL_FOV / 2);
	}

	/**
	 *
	 * @param point
	 * @return
	 */
	inline static float calcAngleJack(const cv::Point2f &point) {
		int a, b;
		double d, alfa;

		b = abs(320 - point.y);
		a = abs((point.x) - 320);
		d = sqrt((a*a)+(b*b));
		alfa = asin (a/d);

		if((point.x) < 320)
			alfa = (360 * -alfa)/6.28;
		else
			alfa = (360 * alfa)/6.28;
		return alfa;
	}

	/**
	 *
	 * @param keypoint
	 * @param label
	 * @param depth
	 * @return
	 */
	static poi::POI* KeyPoint2POIfromDepthMat(const cv::KeyPoint &keypoint, const ObjectType &label, const cv::Mat &depth);

	/**
	 *
	 * @param mutex
	 * @param src
	 * @param rect
	 * @param dest
	 */
	static void imageCutter(cv::Mat src, const cv::Rect &rect, cv::Mat &dest);

	/**
	 *
	 * @param bgr
	 * @param pois
	 */
	static void showPOIs(cv::Mat &bgr, const std::vector<poi::POI*> pois);

	/**
	 *
	 * @param point
	 * @return
	 */
	static float getPOIY(const poi::POI* point);

	/**
	 *
	 * @param point
	 * @return
	 */
	static float getPOIX(const poi::POI* point);

private:

	/*
	 * Previene la costruzione.
	 */
	Utils();

	/*
	 * Essendo vietata la costruzione, non e' necessaria la distruzione.
	 */
	~Utils();

	/*
	 * Previene dalla costruzione mediante copia.
	 */
	Utils(const Utils&);

	/*
	 * Previene l'assegnamento.
	 */
	Utils& operator = (const Utils&);

};

} /* namespace utils */

#endif /* UTILS_H_ */
